#pragma once

#include <map>
#include <optional>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <affx/affine.hpp>
#include <movex/robot/motion_data.hpp>


namespace movex {

struct ImpedanceMotion {
    using Affine = affx::Affine;

    enum class Axis { X, Y, Z };

    enum class Type {
        Cartesian,
        Joint
    };

    enum class TargetMotion {
        Exponential,  // ？
        Linear,
        Spiral,
    };

    struct LinearTargetMotion {  // 线性运动
        Affine relative_target;
        double duration;  // 设置运动用时
        bool initialized {false};
        bool finish_after {true};
        double finish_wait_factor {1.2};
    };

    struct SpiralTargetMotion {  // 螺旋运动
        Affine center; // In XY-plane 螺旋运动的中心点， 在XY平面上
        double revolutions_per_second; // 表示每秒转多少圈， 决定了速度
        double radius_per_revolution;  // 表示每圈半径多少， 决定了半径
        bool initialized {false};
    };

    Type type; // 运动类型：关节运动、笛卡尔运动

    TargetMotion target_motion {TargetMotion::Exponential}; // 目标运动类型
    double exponential_decay {0.005}; // 指数衰减？
    LinearTargetMotion linear_motion; // 线性运动设置
    SpiralTargetMotion spiral_motion; // 螺旋运动设置

    std::map<Axis, double> force_constraints; // 力约束,可分别约束X,Y,Z轴的力

public:
    const double translational_stiffness {2000.0};  // in [10, 3000] N/m
    const double rotational_stiffness {200.0};  // in [1, 300] Nm/rad
    const double joint_stiffness {200.0};  // ?

    Affine target;
    bool is_active {false};
    bool should_finish {false};

    explicit ImpedanceMotion() { }
    explicit ImpedanceMotion(double joint_stiffness): joint_stiffness(joint_stiffness), type(Type::Joint) { }
    explicit ImpedanceMotion(double translational_stiffness, double rotational_stiffness): translational_stiffness(translational_stiffness), rotational_stiffness(rotational_stiffness), type(Type::Cartesian) { }


    Affine getTarget() const {
        return target;
    }

    void setTarget(const Affine& new_target) { // 当 active 时，可以设置新目标
        if (is_active) {
            target = new_target;
        }
        target_motion = ImpedanceMotion::TargetMotion::Exponential; // ？
    }

    // 设置线性 相对 运动
    void setLinearRelativeTargetMotion(const Affine& relative_target, double duration) {
        linear_motion = {relative_target, duration};
        target_motion = ImpedanceMotion::TargetMotion::Linear;
    }

    // 设置螺旋运动
    void setSpiralTargetMotion(const Affine& center, double revolutions_per_second, double radius_per_revolution) {
        spiral_motion = {center, revolutions_per_second, radius_per_revolution};
        target_motion = ImpedanceMotion::TargetMotion::Spiral;
    }

    // 设置某个轴的力约束， 只有在非 active 时才能设置
    void addForceConstraint(Axis axis, double value) {
        if (is_active) {
            return;
        }

        force_constraints[axis] = value;
    }

    // 设置力约束， 只有在非 active 时才能设置
    void addForceConstraint(std::optional<double> x = std::nullopt, std::optional<double> y = std::nullopt, std::optional<double> z = std::nullopt) {
        if (is_active) {
            return;
        }

        if (x) {
            force_constraints[Axis::X] = x.value();
        }
        if (y) {
            force_constraints[Axis::Y] = y.value();
        }
        if (z) {
            force_constraints[Axis::Z] = z.value();
        }
    }

    bool isActive() const {
        return is_active;
    }

    void finish() {
        should_finish = true;
    }
};

} // namespace movex
